﻿namespace RLab.Visualization

open System
open Microsoft.DirectX
open Microsoft.DirectX.Direct3D
open StillDesign.PhysX
open GeometryDefinitions
open CommonDefinitions


[<CompilationRepresentation(CompilationRepresentationFlags.ModuleSuffix)>]
module PhysicChain = 
    begin
    let createPhysicChain engine kinchain = 
        let rec createPhysicSegment segment prevActor =
            match segment with
            | Link(j,l,s) -> 
                let nextActor = PhysicEngine.createCubeActor engine l                
                Link( PhysicEngine.createChainJoint engine j prevActor nextActor,
                     (nextActor,l),
                      createPhysicSegment s nextActor)
            | End -> End
        match kinchain with
        | Chain(b0,s) -> 
            let a = PhysicEngine.createCubeActor engine b0
            PhysicEngine.createFixedJoint engine null a |> ignore            
            Chain( (a,b0),createPhysicSegment s a )
    end

type DriverExecutionException(message) = 
    inherit System.Exception(message)

//Make as generic determined by toolBuilder result (unit->TTool where TTool : ITool)
type PhysicRobot( renderer : Renderer, engine , kinchainDefinition, toolBuilder : unit->ITool ) = 
    let kinChain = PhysicChain.createPhysicChain engine kinchainDefinition
    let tool = toolBuilder()
    let ji = PhysicEngine.createFixedJoint engine (fst (KinematicChain.lastLink kinChain)) tool.End
    do renderer.DrawScene.Add
        ( fun d ->
            let list = kinChain |> KinematicChain.joints |> Seq.toList
            kinChain |> KinematicChain.linkIter
                ( fun l ->
                    d.SetTransform(TransformType.World, Matrix.Translation((snd l).CenterOfMassOffset) * (fst l).GlobalPose  * renderer.MeshScale  )
                    (snd l).Model.DrawSubset(0)))
    member r.Tool = tool
    member r.Move(jointID,angle,velocity) = 
        try
            let joint = 
                kinChain |>
                KinematicChain.firstJoint ( fun j -> j.Name = jointID )
            match joint with
            | :? RevoluteJoint -> 
                joint:?>RevoluteJoint |>                 
                PhysicEngine.moveRevoluteTo engine angle velocity                                
            | :? D6Joint -> 
                joint:?>D6Joint |> 
//todo: invoke proper func determined by drive type
                PhysicEngine.movePrismaticTo engine angle velocity
            | _ -> failwith "Drive is not supported for this joint type"
        with | :? JointNotFoundException -> raise(DriverExecutionException("Driver not found."))

    interface System.IDisposable with
        member r.Dispose() = 
            kinChain |> KinematicChain.linkIter (fun l -> (fst l).Dispose())
            kinChain |> KinematicChain.jointIter (fun j -> j.Dispose())
    end